Test Code For Sensors
#include <xc.h>
#include <stdint.h>
#define _XTAL_FREQ 4000000

// LED indicators (330O resistors used externally)
#define GREEN_LED LATAbits.LATA0
#define RED_LED   LATAbits.LATA1
#define YELLOW_LED LATAbits.LATA2

// I2C addresses
#define MAX30102_ADDR 0x57
#define MLX90614_ADDR 0x5A

void I2C_Init(void)
{
    TRISC3 = 1; // SCL input
    TRISC4 = 1; // SDA input

    SSP1CON1 = 0x28; // I2C Master mode
    SSP1ADD = 9;     // 100kHz clock

    SSP1STAT = 0x00;
}

unsigned int Read_IR_Value(void)
{
    // Placeholder for real FIFO reading
    // Replace later with full MAX30102 driver
    return 52000;
}

void Test_HeartRateSensor(void)
{
    while(1)
    {
        unsigned int ir = Read_IR_Value();

        // If signal exists ? LED ON
        if(ir > 50000)
            GREEN_LED = 1;
        else
            GREEN_LED = 0;

        __delay_ms(200);
    }
}

unsigned int Read_Temp_Raw(void)
{
    // Placeholder raw value
    return 3000;
}

float ConvertTemp(unsigned int raw)
{
    float temp = raw * 0.02;
    return temp - 273.15;
}

void Test_TemperatureSensor(void)
{
    while(1)
    {
        unsigned int raw = Read_Temp_Raw();
        float temp = ConvertTemp(raw);

        // LED ON if temperature detected
        if(temp > 0)
            YELLOW_LED = 1;
        else
            YELLOW_LED = 0;

        __delay_ms(500);
    }
}

char I2C_Check(unsigned char addr)
{
    SSP1CON2bits.SEN = 1;
    while(SSP1CON2bits.SEN);

    SSP1BUF = (addr << 1);
    while(!PIR1bits.SSP1IF);
    PIR1bits.SSP1IF = 0;

    char ack = !SSP1CON2bits.ACKSTAT;

    SSP1CON2bits.PEN = 1;
    while(SSP1CON2bits.PEN);

    return ack;
}

void Test_I2C_Devices(void)
{
    while(1)
    {
        if(I2C_Check(MAX30102_ADDR))
            GREEN_LED = 1;
        else
            GREEN_LED = 0;

        if(I2C_Check(MLX90614_ADDR))
            YELLOW_LED = 1;
        else
            YELLOW_LED = 0;

        __delay_ms(300);
    }
}

unsigned int bpm = 0;
unsigned int last_peak = 0;
unsigned int current_time = 0;

char DetectPeak(unsigned int value)
{
    static unsigned int prev = 0;

    if(value > 50000 && prev <= 50000)
    {
        prev = value;
        return 1;
    }

    prev = value;
    return 0;
}

void Test_BPM(void)
{
    while(1)
    {
        unsigned int ir = Read_IR_Value();

        if(DetectPeak(ir))
        {
            current_time += 100;

            if(last_peak != 0)
            {
                bpm = 60000 / (current_time - last_peak);
            }

            last_peak = current_time;
        }

        // RED LED if BPM too high or too low
        if(bpm > 85)
            RED_LED = 1;
	if(bpm <60)
	   RED_LED = 1;
        else
            RED_LED = 0;

        __delay_ms(200);
    }
}

void Test_Temp_Threshold(void)
{
    while(1)
    {
        float temp = ConvertTemp(Read_Temp_Raw());

        if(temp > 38.0)
            YELLOW_LED = 1;
        else
            YELLOW_LED = 0;

        __delay_ms(500);
    }
}

void main(void)
{
    I2C_Init();

    TRISAbits.TRISA0 = 0;
    TRISAbits.TRISA1 = 0;
    TRISAbits.TRISA2 = 0;

    GREEN_LED = RED_LED = YELLOW_LED = 0;

    // UNCOMMENT ONLY ONE TEST:

    // Test_HeartRateSensor();
    // Test_TemperatureSensor();
    // Test_I2C_Devices();
    // Test_BPM();
    Test_Temp_Threshold();
}
